#include "problem.h"

#include <Eigen/Geometry>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

using namespace ceres;

DataIntersection absolute_orientation(const DataAbsoluteOrientation &data_ao) {

    DataIntersection result;

    // 1. 利用同名点数据，生成相对定向结果
    Vector3d B;
    Matrix3d Rr;
    DataRelativeOrientation data_ro;
    data_ro.Bx = data_ao.Bx;
    data_ro.tiepoints = data_ao.tiepoints;
    std::tie(B, Rr) = relative_orientation(data_ro);

    // 2. 根据相对定向结果和同名点，计算模型坐标，即在第一个影像坐标系下进行前方交会
    std::vector<Vector3d> model_points(data_ao.tiepoints.size());
    
    // 3. 根据前方交会的模型坐标和控制点，计算旋转，缩放，平移参数
    double s = 1.0;
    Vector3d ra = Vector3d::Zero();
    Vector3d T = Vector3d::Zero();

    // 3.1 计算尺度、缩放、平移的初始值

    // 3.2 构造最小二乘优化，利用控制点和模型坐标，计算7参数

    // 4. 根据 7 参数信息，生成最后的外方位元素
    result.R_eo[0] = rodrigues(ra);
    result.X_eo[0] = T;
    result.R_eo[1] = rodrigues(ra) * Rr;
    result.X_eo[1] = T + s * rodrigues(ra) * B;

    return result;
}
